import math
import csv


def rad2deg(rad):
    return rad * 180 / math.pi


def deg2rad(deg):
    return deg * math.pi / 180


class IMU:
    def __init__(self):
        self.n = 200  # 参与零偏计算的数据大小
        self.t_raw_data = []
        self.roll_raw = []
        self.pitch_raw = []
        self.yaw_raw = []
        self.acc_raw_data = []  # 从文件读取的加速度计原始数据[m/s^2]
        self.gyro_raw_data = []  # 从文件读取的陀螺仪原始数据[rad/s]
        self.acc_data = []  # 加速度计去除零偏后的输出[ax, ay, az] 单位：[m/s^2]
        self.gyro_data = []  # 陀螺仪减去零偏后的输出[gx, gy, gz]  单位：[rad/s]
        self.acc_hat_angle = []  # 加速度计对姿态的估计值[roll, pitch] 单位：[rad]
        print("IMU初始化")

    def read_csv_file(self, filename):
        # gyro_temp = []
        with open(filename) as f:
            reader = csv.reader(f)  # 创建csv文件读取器
            next(reader)  # 去除第一行的数据
            for row in reader:
                self.t_raw_data.append(float(row[0]))
                self.acc_raw_data += list(map(float, row[1:4]))  # 将string数组转成float数组
                self.gyro_raw_data += list(map(float, row[4:7]))
                self.roll_raw.append(float(row[7]))
                self.pitch_raw.append(float(row[8]))
                self.yaw_raw.append(float(row[9]))
                # gyro_temp += list(map(float, row[4:7]))
            f.close()
            # self.gyro_raw_data = [deg2rad(i) for i in gyro_temp]  # 将原始数据deg/s转成rad/s

    def get_acc_attitude_estimate(self):
        """
        @brief:获取加速计的姿态估计
        """
        self.get_acc_data()
        for i in range(int(len(self.acc_data) / 3)):
            ax = self.acc_data[3 * i]  # 加速度计x轴输出
            ay = self.acc_data[3 * i + 1]
            az = self.acc_data[3 * i + 2]
            self.acc_hat_angle.append(math.atan2(ay, az))  # roll估计值)
            self.acc_hat_angle.append(math.atan2(-ax, math.sqrt(ay ** 2.0 + az ** 2.0)))  # pitch估计值
        return self.acc_hat_angle

    def get_acc_data(self):
        """
        @brief:获取加速度计减去零偏之后的输出
        """
        # 零偏值来源于静止下的计算，参考cal_imu_zero_bias.py
        ax_bias = 0.246953
        ay_bias = -0.015280
        az_bias = 0.009261
        # 计算减去零偏值后的加速度计的输出, 然后乘以旋转矩阵
        for i in range(int(len(self.acc_raw_data) / 3)):
            ax_temp = self.acc_raw_data[3 * i] - ax_bias
            ay_temp = self.acc_raw_data[3 * i + 1] - ay_bias
            az_temp = self.acc_raw_data[3 * i + 2] - az_bias
            self.acc_data.append(-ay_temp)
            self.acc_data.append(ax_temp)
            self.acc_data.append(az_temp)

        return self.acc_data

    def get_gyro_data(self):
        """
        @brief:获取陀螺仪减去零偏后的输出
        """
        # 零偏值来源于静止下的计算，参考cal_imu_zero_bias.py
        gx_bias = 0.000453
        gy_bias = 0.000453
        gz_bias = 0.000453
        # 计算减去零偏值后陀螺仪的输出
        for i in range(int(len(self.gyro_raw_data) / 3)):
            gx_temp = self.gyro_raw_data[3 * i] - gx_bias  # 陀螺仪输出gx = 陀螺仪原始输出-零偏[rad/s]
            gy_temp = self.gyro_raw_data[3 * i + 1] - gy_bias
            gz_temp = self.gyro_raw_data[3 * i + 2] - gz_bias
            self.gyro_data.append(-gy_temp)
            self.gyro_data.append(gx_temp)
            self.gyro_data.append(gz_temp)
        return self.gyro_data
